/*****************************************************************************
* File Name: motion_sensor.c
*
* Version: 1.0
*
* Description:
* This file contains API for motion sensor MPU6500  
*
* Note:
* The functionality of the motion sensor will be available in the next release.
*
* Owner: KMVP
*
* Related Document:
* SCB component datasheet
*
* Hardware Dependency:
* MPU6500 Datasheet
*
* Code Tested With:
* 1. PSoC Creator 3.1 
* 2. ARM GCC 4.8.4
******************************************************************************
* Copyright (2015), Cypress Semiconductor Corporation.
******************************************************************************
* This software is owned by Cypress Semiconductor Corporation (Cypress) and is
* protected by and subject to worldwide patent protection (United States and
* foreign), United States copyright laws and international treaty provisions.
* Cypress hereby grants to licensee a personal, non-exclusive, non-transferable
* license to copy, use, modify, create derivative works of, and compile the
* Cypress Source Code and derivative works for the sole purpose of creating
* custom software in support of licensee product to be used only in conjunction
* with a Cypress integrated circuit as specified in the applicable agreement.
* Any reproduction, modification, translation, compilation, or representation of
* this software except as specified above is prohibited without the express
* written permission of Cypress.
*
* Disclaimer: CYPRESS MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, WITH
* REGARD TO THIS MATERIAL, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 
* Cypress reserves the right to make changes without further notice to the
* materials described herein. Cypress does not assume any liability arising out
* of the application or use of any product or circuit described herein. Cypress
* does not authorize its products for use as critical components in life-support
* systems where a malfunction or failure may reasonably be expected to result in
* significant injury to the user. The inclusion of Cypress' product in a life-
* support systems application implies that the manufacturer assumes all risk of
* such use and in doing so indemnifies Cypress against all charges. Use may be
* limited by and subject to the applicable Cypress software license agreement.
*****************************************************************************/
#include "motion_sensor.h"
#include "motion_sensor_hal.h"
#include "AIR_MOTION_Lib.h"

#ifndef DISABLE_MOTION_SENSOR

#include "bcp.h"
typedef struct _Gain_Resolution_
{
    uint8 deltaXGain;
    uint8 deltaYGain;
}Gain_Resolution;

bool isCalibrationDone = false;

/* Global variables those would be updated by motion sensor polling routine*/
static int8 deltaX;
static int8 deltaY;

uint8 MotionSensorState = 1;

static t_struct_AIR_MOTION_Init  lInitParameters = 
{
    /* Delta X and Y Gain */
    {AIR_MOTION_DEFAULT_GAIN_DELTA_X, AIR_MOTION_DEFAULT_GAIN_DELTA_Y},
    /* Gyro Offset */
    {AIR_MOTION_DEFAULT_GYROOFFSET_X,AIR_MOTION_DEFAULT_GYROOFFSET_Y,AIR_MOTION_DEFAULT_GYROOFFSET_Z},
    /* GyroStatic Noise */
    AIR_MOTION_DEFAULT_GYROSTATIC_NOISE,
    /* StaticSamples */
    AIR_MOTION_DEFAULT_STATIC_SAMPLES,
    /* SwipeDistMin */
    AIR_MOTION_DEFAULT_SWIPE_DISTMIN,
    /* SwipeNoiseMax */ 
    AIR_MOTION_DEFAULT_SWIPE_NOISE_MAX,
    /* NbSamplesGyroStartup */
    AIR_MOTION_DEFAULT_SAMPLES_GYRO_STARTUP,
    /* ClickStillSamples */
    AIR_MOTION_DEFAULT_CLICKSTILLSAMPLES,
    /* ClickStillTolerance */
    AIR_MOTION_DEFAULT_CLICKSTILLTOLERANCE,
    /* IsRollCompEnabled */
    AIR_MOTION_DEFAULT_ISROLLCOMPENABLED,
    /* AccNbLsb1G */
    AIR_MOTION_DEFAULT_ACCR_SENSITIVITY,
    /* GyroSensitivity */
    AIR_MOTION_DEFAULT_GYRO_SENSITIVITY
};

extern uint16 BcpBuf[30];

static void Motion_Sensor_Flash_Write_Offsets(void);

/*****************************************************************************
* Function Name: Motion_Sensor_CallBack()
******************************************************************************
* Summary:
* This functions is a callback function when motion sensor interrupt triggers
*
* Parameters:
* None
*
* Return:
* None
*
* Side Effects:
* None
* 
* Note:
* None
*****************************************************************************/
static void Motion_Sensor_CallBack(void)
{
    DEVICE_ACTIVITY_DETECTED();
}

/*****************************************************************************
* Function Name: Motion_Sensor_Init()
******************************************************************************
* Summary:
* This function initializes motion sensor. 
*
* Parameters:
* None
*
* Return:
* None
*
* Theory
* The initialization puts the sensor into low power mode. This saves power by
* disabling the sensor till data sampling is started
*
* Side Effects:
* isCalibrationDone is updated
* 
* Note:
* On power up Motion sensor is put into low power mode 
*****************************************************************************/
void Motion_Sensor_Init(void)
{   
    Motion_Sensor_Hal_Init(Motion_Sensor_CallBack);
#ifdef ENABLE_MOTION_SENSOR_FLASH_WRITE
    Device_Flash_Restore((uint8 *)&lInitParameters.GyroOffsets.X, (uint8 *)&flashDataInRam.motionGyroOffest, \
                                                                                sizeof(t_struct_AIR_MOTION_SensorAxis));
    /* check if the Gyro offset stored in flash are different than default values.
     * If yes, calibration is already done 
     */
    if((lInitParameters.GyroOffsets.X != AIR_MOTION_DEFAULT_GYROOFFSET_X) || 
       (lInitParameters.GyroOffsets.Y != AIR_MOTION_DEFAULT_GYROOFFSET_Y) || 
       (lInitParameters.GyroOffsets.Z != AIR_MOTION_DEFAULT_GYROOFFSET_Z))
    {
        isCalibrationDone = true;
    }
#endif /* ENABLE_MOTION_SENSOR_FLASH_WRITE */
    AIR_MOTION_Init(&lInitParameters);
    Motion_Sensor_Stop_Sampling();
}

/*****************************************************************************
* Function Name: Motion_Sensor_Set_Resolution()
******************************************************************************
* Summary:
* This function updates the resolution of mouse movement
*
* Parameters:
* uint8 gainResolutionIndex - index of the resolution set to be used
*
* Return:
* None
*
* Theory
* Updates the resolution of mouse movement by updating lInitParameters and 
* reinitializing Air Motion library
*
* Side Effects:
* Reinitializes AIR Motion library results in mouse movement seizure for 
* "StartupSamples" ( a parameter of lInitParameters) number of samples.
* 
* Note:
* gainResolutionIndex can take value from 0,1,2,3. 
* Following are the values the resolution it takes
* -----------------------------------------
* gainResolutionIndex    |    Resolution  |
* -----------------------------------------
*         0              |      2         |
*         1              |      7         |
*         2              |      10        |
*         3              |      15        | (Full Resolution)
* -----------------------------------------
*****************************************************************************/
void Motion_Sensor_Set_Resolution(uint8 gainResolutionIndex)
{    
    const Gain_Resolution gainResolutionTable[] = 
    {
        {2,2},
        {7,7},
        {10,10},
        {15, 15}
    };

    lInitParameters.DeltaGain.X          = gainResolutionTable[gainResolutionIndex].deltaXGain;
    lInitParameters.DeltaGain.Y          = gainResolutionTable[gainResolutionIndex].deltaYGain;
    AIR_MOTION_Init(&lInitParameters);
}

/*****************************************************************************
* Function Name: Motion_Sensor_Start_Sampling()
******************************************************************************
* Summary:
* This function starts the sampling of motion sensor data
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* MotionSensorState is updated
*
* Note: 
* None
*****************************************************************************/
void Motion_Sensor_Start_Sampling(void)
{
    if(!MotionSensorState)
    {
        MotionSensorState = 1;
        /* Start the sampling */
        Motion_Sensor_Hal_Start();
    }
}

/*****************************************************************************
* Function Name: Motion_Sensor_Stop_Sampling()
******************************************************************************
* Summary:
* This function is stops the sampling the motion sensor data
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* MotionSensorState is updated
*
* Note: 
* Gyroscope offsets stored in flash are also updated
*****************************************************************************/
void Motion_Sensor_Stop_Sampling(void)
{
    if(MotionSensorState)
    {
        MotionSensorState = 0;
        Motion_Sensor_Hal_Stop();
        Motion_Sensor_Flash_Write_Offsets();
    }
}

/*****************************************************************************
* Function Name: Motion_Sensor_Stop()
******************************************************************************
* Summary:
* This function stops motion sensor. 
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* Stops the motion sensor by disabling the low power mode and stoping the sampling
*
* Side Effects:
* None
*
* Note: 
* None
*****************************************************************************/
void Motion_Sensor_Stop(void)
{
    Motion_Sensor_Hal_Disable_LowPowerMode();
    Motion_Sensor_Stop_Sampling();
    Motion_Sensor_Flash_Write_Offsets();
}

/*****************************************************************************
* Function Name: Motion_Sensor_Flash_Write_Offsets()
******************************************************************************
* Summary:
* This function saves the Gyroscope offsets in flash.
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* The offets are written to flash only if the offset value has changed by significant
* amount. This amount of change is defines as AIR_MOTION_REGULAR_STATIC_SAMPLES
* 
* Side Effects:
* None
* 
* Note:
* None
*****************************************************************************/
static void Motion_Sensor_Flash_Write_Offsets(void)
{
#ifdef ENABLE_MOTION_SENSOR_FLASH_WRITE
    t_struct_AIR_MOTION_SensorAxis storedflashGyroOffset;
	/* read the existing offsets in flash and store them in SRAM*/
    Device_Flash_Restore((uint8 *)&storedflashGyroOffset.X, (uint8 *)&flashDataInRam.motionGyroOffest, \
                                                                    sizeof(t_struct_AIR_MOTION_SensorAxis));
    if(((storedflashGyroOffset.X + (int16)MOTION_SENSOR_GYRO_OFFSET_THRESHOLD_FLASH_WRITE) <  lInitParameters.GyroOffsets.X) || 
       ((storedflashGyroOffset.X - (int16)MOTION_SENSOR_GYRO_OFFSET_THRESHOLD_FLASH_WRITE) > lInitParameters.GyroOffsets.X ) || 
       ((storedflashGyroOffset.Y + (int16)MOTION_SENSOR_GYRO_OFFSET_THRESHOLD_FLASH_WRITE) < lInitParameters.GyroOffsets.Y) || 
       ((storedflashGyroOffset.Y - (int16)MOTION_SENSOR_GYRO_OFFSET_THRESHOLD_FLASH_WRITE) > lInitParameters.GyroOffsets.Y) ||
       ((storedflashGyroOffset.Z + (int16)MOTION_SENSOR_GYRO_OFFSET_THRESHOLD_FLASH_WRITE) < lInitParameters.GyroOffsets.Z) || 
       ((storedflashGyroOffset.Z - (int16)MOTION_SENSOR_GYRO_OFFSET_THRESHOLD_FLASH_WRITE) > lInitParameters.GyroOffsets.Z))
    {
       Device_Flash_Write( (uint8 *)&flashDataInRam.motionGyroOffest, (uint8 *)&lInitParameters.GyroOffsets.X, \
                                                                                    sizeof(t_struct_AIR_MOTION_SensorAxis));
    }
#endif /* ENABLE_MOTION_SENSOR_FLASH_WRITE */
}

/*****************************************************************************
* Function Name: Motion_Sensor_Set_State()
******************************************************************************
* Summary:
* This function is used to set the state of the motion sensor in sampling / motion 
* detect mode
*
* Parameters:
* Device_State state - the state to set
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note: 
* None
*****************************************************************************/
void Motion_Sensor_Set_State(Device_State state)
{
    if(MotionSensorState)
    {
        if(state == DEVICE_ACTIVE)
        {
            Motion_Sensor_Hal_Disable_LowPowerMode();
        }
        else
        {
            Motion_Sensor_Hal_Enable_LowPowerMode();
            Motion_Sensor_Flash_Write_Offsets();
        }
    }
}

/*****************************************************************************
* Function Name: MotionSensor_IsMotionDetectedLowPower()
******************************************************************************
* Summary:
* This function is used to poll for the motion sensor activity in low power mode
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* Polling the motion sensor will increase current consumption 
*
* Note: 
* This function should be used only when interrupt based wake up is not possible
*****************************************************************************/
void MotionSensor_IsMotionDetectedLowPower(void)
{
    if(Motion_Sensor_Hal_IsMotionDetected())
    {
        DEVICE_ACTIVITY_DETECTED();
    }
}

/*****************************************************************************
* Function Name: Motion_Sensor_IsActive()
******************************************************************************
* Summary:
* This function is used check if the motion sensor is active or not
*
* Parameters:
* None
*
* Return:
* bool - true if motion sensor is active and shows mouse pointer movement
*
* Theory:
*
* Side Effects:
* None
*
* Note: 
* None
*****************************************************************************/
bool Motion_Sensor_IsActive(void)
{
    if(MotionSensorState)
    {
        if((deltaX != 0) || (deltaY != 0))
        {
            return true;
        }
    }
    return false;
}

/*****************************************************************************
* Function Name: Motion_Sensor_Poll()
******************************************************************************
* Summary:
* This function is used to check if motion sensor has mouse pointer movement
*
* Parameters:
* bool buttonChangedStatus - true if the button status has pressed
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* Gyroscopes offsets may get updated and written in to flash. 
*
* Note:
* None
*****************************************************************************/
void Motion_Sensor_Poll(bool buttonChangedStatus)
{
    t_struct_AIR_MOTION_ProcessDeltaStatus lProcessDeltaStatus;
    t_struct_AIR_MOTION_ProcessDeltaSamples lSensorSamples;

    if(MotionSensorState)
    {
        /* Read the sensors */
        Motion_Sensor_Hal_GetGyroData((int16 *)&lSensorSamples.GyroSamples.X);
        Motion_Sensor_Hal_GetAccData((int16 *)&lSensorSamples.AccSamples.X);
        
        /* Report button presses to the library.
         * It will be used by click tolerance logic of the library
         */
        lSensorSamples.ClickSample = buttonChangedStatus;
        lProcessDeltaStatus = AIR_MOTION_ProcessDelta(lSensorSamples);
        
        if(lProcessDeltaStatus.Status.IsDeltaComputed == true)
        {
            deltaX = lProcessDeltaStatus.Delta.X;
            deltaY = lProcessDeltaStatus.Delta.Y;
        }
        else
        {
            deltaX = 0;
            deltaY = 0;
        }
#ifdef ENABLE_BSP
						Bcp_Start();
						
						BcpBuf[0] = (lSensorSamples.AccSamples.X);
						BcpBuf[1] = (lSensorSamples.AccSamples.Y);
						BcpBuf[2] = (lSensorSamples.AccSamples.Z);

						BcpBuf[3] = lSensorSamples.GyroSamples.X;
						BcpBuf[4] = lSensorSamples.GyroSamples.Y;
						BcpBuf[5] = lSensorSamples.GyroSamples.Z;

						Bcp_Show(BcpBuf,6); 
#else
          if(deltaX || deltaY)
          {
            Debug_Print(DEBUG_MESSAGE_LEVEL_5,"deltaX=%d,deltaY=%d\r\n",deltaX,deltaY);
          }
#endif

        /* If new offset are computed save them to lInitParameters */
        if(lProcessDeltaStatus.Status.NewGyroOffset == true)
        {
            lInitParameters.GyroOffsets.X = lProcessDeltaStatus.GyroOffsets.X;
            lInitParameters.GyroOffsets.Y = lProcessDeltaStatus.GyroOffsets.Y;
            lInitParameters.GyroOffsets.Z = lProcessDeltaStatus.GyroOffsets.Z;
            isCalibrationDone = true;
            if(lInitParameters.StaticSamples != AIR_MOTION_REGULAR_STATIC_SAMPLES)
            {
                lInitParameters.StaticSamples = AIR_MOTION_REGULAR_STATIC_SAMPLES;
                AIR_MOTION_Init(&lInitParameters);
            }
        }
    }

    if(isCalibrationDone && ((deltaX != 0) || (deltaY != 0)))
    {
        DEVICE_POST_EVENT(MOTION_EVENT);
    }
}

/*****************************************************************************
* Function Name: Motion_Sensor_Get_Report()
******************************************************************************
* Summary:
* This function    uses the mouse position shift to update the HID report
* passed as parameter
*
* Parameters:
* report : pointer to the report to be updated
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
* 
* Note:
* None
*****************************************************************************/
ReportType Motion_Sensor_Get_Report(Report *report)
{
    ReportType reportType = NO_REPORT;
    if(DEVICE_GET_EVENT(MOTION_EVENT))
    {
        if((deltaX != 0) || (deltaY != 0))
        {
            report->mouse_report.x = deltaX;
            report->mouse_report.y =  deltaY;     
            reportType |= MOUSE_REPORT;
        }
    }
    return reportType;  
}

#endif /* DISABLE_MOTION_SENSOR */
